#include "camera.h"
#include "native_debug.h"

// basic tools
float r3d_depth_image::normalize(cv::Point3f & v) {
	float len_ = v.x*v.x + v.y*v.y + v.z*v.z;
	if (len_ > 0) v /= std::sqrtf(len_);
	return len_;
}


bool r3d_depth_image::check_inversion(
	const cv::Mat & mat1, 
	const cv::Mat & mat2
	) {
	if (mat1.type() != CV_32FC1) {
		IMAGE_DEPTH_LOGE("[check_inversion] unsupported type!");
		FAIL_EXIT(-1);
	}
	cv::Mat tmp_ = mat1 * mat2;
	int n = tmp_.size().width;
	
	if (!tmp_.isContinuous()) {
		IMAGE_DEPTH_LOGE("[check_inversion] memory discontinuous!");
		FAIL_EXIT(-1);
	}
	float * pT = (float*)(tmp_.data);
	
	for (int i = 0; i < n; ++i) {
		for (int j = 0; j < n; ++j) {
			if (!((i == j && std::fabs(pT[j]-1) < 1e-5) || \
				(i != j && std::fabs(pT[j]) < 1e-5))) return false;
		}
		pT += n;
	}
	return true;
}


cv::Point3f r3d_depth_image::rotate_vector(
	const cv::Point3f & vec, 
	const cv::Mat & rot
	) {
	cv::Point3f out_;
	if (!rot.isContinuous()){
		IMAGE_DEPTH_LOGE("Jesus!");
		FAIL_EXIT(-1);
	}
	float * pR = (float*)(rot.data);
	out_.x = pR[0] * vec.x + pR[1] * vec.y + pR[2] * vec.z;
	out_.y = pR[3] * vec.x + pR[4] * vec.y + pR[5] * vec.z;
	out_.z = pR[6] * vec.x + pR[7] * vec.y + pR[8] * vec.z;
	return out_;
}


cv::Mat r3d_depth_image::get_axis_rot(
	const cv::Point3f & axis, 
	float degree
	) {
	if (axis == cv::Point3f(0, 0, 0)) {
		IMAGE_DEPTH_LOGE("[get_axis_rot] axis being zero-vector!");
		FAIL_EXIT(-1);
	}
	cv::Point3f axis_ = axis;
	// normalize axis vector
	if (CHECK_ZERO(normalize(axis_))) {
		IMAGE_DEPTH_LOGE("[get_axis_rot] norm of axis is zero!");
		FAIL_EXIT(-1);
	}

	float n1 = axis_.x;
	float n2 = axis_.y;
	float n3 = axis_.z;

	// get the rotation angle [A] with cosA and sinA
	degree *= PI / 180.0;
	float cosA = std::cos(degree);
	float sinA = std::sin(degree);

	// calculate the direct rotation matrix
	//           [cosA+n1^2*(1-cosA) n1*n2*(1-cosA)-n3*sinA n1*n3*(1-cosA)+n2*sinA]
	// R(n, A) = [n1*n2*(1-cosA)+n3*sinA cosA+n2^2*(1-cosA) n2*n3*(1-cosA)-n1*sinA]
	//	         [n1*n3*(1-cosA)-n2*sinA n2*n3*(1-cosA)+n1*sinA cosA+n3^2*(1-cosA)]
	cv::Mat rot_(cv::Size(3, 3), CV_32FC1);
	rot_.at<float>(0, 0) = cosA + n1*n1 * (1 - cosA);
	rot_.at<float>(1, 0) = n1*n2*(1 - cosA) - n3*sinA;
	rot_.at<float>(2, 0) = n1*n3*(1 - cosA) + n2*sinA;

	rot_.at<float>(0, 1) = n1*n2*(1 - cosA) + n3*sinA;
	rot_.at<float>(1, 1) = cosA + n2*n2 * (1 - cosA);
	rot_.at<float>(2, 1) = n2*n3*(1 - cosA) - n1*sinA;

	rot_.at<float>(0, 2) = n1*n3*(1 - cosA) - n2*sinA;
	rot_.at<float>(1, 2) = n2*n3*(1 - cosA) + n1*sinA;
	rot_.at<float>(2, 2) = cosA + n3*n3 * (1 - cosA);

	return rot_;
}


void r3d_depth_image::update_rot_by_orts(
	cv::Mat & rot,
	const cv::Point3f & ort_x,
	const cv::Point3f & ort_y,
	const cv::Point3f & ort_z
	) {
	if (rot.type() != CV_32FC1 || rot.size() != cv::Size(3, 3)) {
		rot.create(cv::Size(3, 3), CV_32FC1);
	}

	// compose [n1|n2|n3]
	rot.at<float>(0, 0) = ort_x.x;
	rot.at<float>(0, 1) = ort_x.y;
	rot.at<float>(0, 2) = ort_x.z;
	rot.at<float>(1, 0) = ort_y.x;
	rot.at<float>(1, 1) = ort_y.y;
	rot.at<float>(1, 2) = ort_y.z;
	rot.at<float>(2, 0) = ort_z.x;
	rot.at<float>(2, 1) = ort_z.y;
	rot.at<float>(2, 2) = ort_z.z;
}


r3d_depth_image::Camera::Camera(
	const cv::Point3f & pos, // position
	const cv::Point3f & ort_x, // orientation of X
	const cv::Point3f & ort_y, // orientation of Y
	const cv::Mat & cmm, // camera matrix
	const cv::Size size // size of captured picture
	) {
	m_pos = pos;

	// normalize the 
	float norm_ = ort_x.x*ort_x.x + ort_x.y*ort_x.y + ort_x.z*ort_x.z;
	if (norm_ == 0) {
		IMAGE_DEPTH_LOGE("[Camera] ort_x is a zero vector!");
		FAIL_EXIT(-1);
	}
	m_ort_x = ort_x / norm_;

	norm_ = ort_y.x*ort_y.x + ort_y.y*ort_y.y + ort_y.z*ort_y.z;
	if (norm_ == 0) {
		IMAGE_DEPTH_LOGE("[Camera] ort_y is a zero vector!");
		FAIL_EXIT(-1);
	}
	m_ort_y = ort_y / norm_;

	if (ort_x.dot(ort_y) != 0) {
		IMAGE_DEPTH_LOGE("[Camera] ort_x not perpendicular to ort_y");
		FAIL_EXIT(-1);
	}

	m_ort_z = m_ort_x.cross(m_ort_y);

	// update the rotation matrix
	// according to equation
	// R [n1|n2|n3] = E, where n1, n2, n3 are camera orientation vectors
	// R = [n1|n2|n3]^-1
	// btw, t = -(x_cam, y_cam, z_cam)
	// transform begins with translation, ends with rotation.

	// compose [n1|n2|n3]
	m_rot.create(cv::Size(3, 3), CV_32FC1);
	m_rot.at<float>(0, 0) = m_ort_x.x;
	m_rot.at<float>(0, 1) = m_ort_x.y;
	m_rot.at<float>(0, 2) = m_ort_x.z;

	m_rot.at<float>(1, 0) = m_ort_y.x;
	m_rot.at<float>(1, 1) = m_ort_y.y;
	m_rot.at<float>(1, 2) = m_ort_y.z;

	m_rot.at<float>(2, 0) = m_ort_z.x;
	m_rot.at<float>(2, 1) = m_ort_z.y;
	m_rot.at<float>(2, 2) = m_ort_z.z;

	// compute [n1|n2|n3]^-1
	cv::Mat inv_;
	if (!cv::invert(m_rot, inv_, cv::DECOMP_LU)) {
		// this code will never be reached?
		IMAGE_DEPTH_LOGE("[Camera] inv mat failed at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	if (!check_inversion(m_rot, inv_)) {
		IMAGE_DEPTH_LOGE("[Camera] bad inv mat at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	inv_.copyTo(m_rot);

	cmm.copyTo(m_cmm);
	m_view = cv::Mat::zeros(size, CV_8UC3);
	m_view_depth = cv::Mat::zeros(size, CV_32FC1);
	
	m_bound = false;
	m_view_lock = FREE;
}


// setters
void r3d_depth_image::Camera::SetCameraMatrix(const cv::Mat & cmm) {
	cmm.copyTo(m_cmm);
}


void r3d_depth_image::Camera::SetPosition(cv::Point3f pos) {
	m_pos = pos;
	if (m_bound) LookAt(m_tar);
}


// [*] disabled in bound state
void r3d_depth_image::Camera::SetOrientation(
	cv::Point3f ort_x,
	cv::Point3f ort_y
	) {
	if (m_bound) return;
	// normalize the 
	float norm_ = ort_x.x*ort_x.x + ort_x.y*ort_x.y + ort_x.z*ort_x.z;
	if (norm_ == 0) {
		IMAGE_DEPTH_LOGE("[Camera] ort_x is a zero vector!");
		FAIL_EXIT(-1);
	}
	m_ort_x = ort_x / norm_;

	norm_ = ort_y.x*ort_y.x + ort_y.y*ort_y.y + ort_y.z*ort_y.z;
	if (norm_ == 0) {
		IMAGE_DEPTH_LOGE("[Camera] ort_y is a zero vector!");
		FAIL_EXIT(-1);
	}
	m_ort_y = ort_y / norm_;

	if (ort_x.dot(ort_y) != 0) {
		IMAGE_DEPTH_LOGE("[Camera] ort_x not perpendicular to ort_y");
		FAIL_EXIT(-1);
	}

	m_ort_z = m_ort_x.cross(m_ort_y);
	
	// update the rotation matrix
	// compose [n1|n2|n3]
	if (m_rot.type() != CV_32FC1 || m_rot.size() != cv::Size(3, 3))
		m_rot.create(cv::Size(3, 3), CV_32FC1);
	
	m_rot.at<float>(0, 0) = m_ort_x.x;
	m_rot.at<float>(0, 1) = m_ort_x.y;
	m_rot.at<float>(0, 2) = m_ort_x.z;

	m_rot.at<float>(1, 0) = m_ort_y.x;
	m_rot.at<float>(1, 1) = m_ort_y.y;
	m_rot.at<float>(1, 2) = m_ort_y.z;

	m_rot.at<float>(2, 0) = m_ort_z.x;
	m_rot.at<float>(2, 1) = m_ort_z.y;
	m_rot.at<float>(2, 2) = m_ort_z.z;

	// compute [n1|n2|n3]^-1
	cv::Mat inv_;
	if (!cv::invert(m_rot, inv_, 0)) {
		// this code will never be reached?
		IMAGE_DEPTH_LOGE("[camera] fail to invert mat at %d", __LINE__);
		FAIL_EXIT(-1);
	}
	if (!check_inversion(m_rot, inv_)) {
		IMAGE_DEPTH_LOGE("[Camera] bad inv mat at %d", __LINE__);
		FAIL_EXIT(-1);
	}
	inv_.copyTo(m_rot);
}


void r3d_depth_image::Camera::SetPictureSize(const cv::Size & size) {
	if (m_view.size() != size) {
		while (m_view_lock == TAKEN);
		m_view_lock = TAKEN;
		m_view = cv::Mat::zeros(size, CV_8UC3);
		m_view_depth = cv::Mat::zeros(size, CV_32FC1);
		m_view_lock = FREE;
	}
}


void r3d_depth_image::Camera::SetBoundState(bool state) {
	if (state && !m_bound) {
		if (m_tar == cv::Point3f(0, 0, 0)) {
			IMAGE_DEPTH_LOGE("[SetBoundState] cannot bind camera to (0,0,0)!");
			FAIL_EXIT(1);
		}
		LookAt(m_tar);
	}
	m_bound = state;
}


// getters
void r3d_depth_image::Camera::GetCameraMatrix(cv::Mat & cmm) {
	m_cmm.copyTo(cmm);
}


cv::Point3f r3d_depth_image::Camera::GetPosition() {
	return m_pos;
}


cv::Point3f r3d_depth_image::Camera::GetOrientationX() {
	return m_ort_x;
}


cv::Point3f r3d_depth_image::Camera::GetOrientationY() {
	return m_ort_y;
}


cv::Size r3d_depth_image::Camera::GetPictureSize() {
	return m_view.size();
}


bool r3d_depth_image::Camera::IsBoundToTarget() {
	return m_bound;
}


void r3d_depth_image::Camera::GetLastFrame(cv::Mat & frame) {
	m_view.copyTo(frame);
}


cv::Point3f r3d_depth_image::Camera::GetTargetPosition() {
	return m_tar;
}


void r3d_depth_image::Camera::CapturePoints(
	const cv::Mat & world, // XYZ(RGBA)
	cv::Mat & view,
	cv::Mat & depth) {
	// reuse buffer as possible
	if (view.type() != CV_8UC3 || view.size() != m_view.size()) {
		view = cv::Mat::zeros(m_view.size(), CV_8UC3);
	}
	else {
		// make it black
		if (view.isContinuous()) {
			memset((uint8_t*)(view.data), 0, view.channels()*view.size().area());
		}
		else {
			view = cv::Mat::zeros(m_view.size(), CV_8UC3);
		}
	}
	if (depth.type() != CV_32FC1 || depth.size() != m_view_depth.size()) {
		depth = cv::Mat::zeros(m_view_depth.size(), CV_32FC1);
	}
	else {
		// make it black
		if (depth.isContinuous()) {
			float * ptr_ = (float*)(depth.data);
			std::fill(ptr_, ptr_ + depth.size().area(), 0.0);
		}
		else {
			depth = cv::Mat::zeros(m_view_depth.size(), CV_32FC1);
		}
	}

	// calculate the crossing point on the canvas, of the ray,
	// from point in the cloud to the optical center of camera
	// the optical center is the position of camera
	if (!world.isContinuous()) {
		IMAGE_DEPTH_LOGE("[CapturePoints] world is discontinuous in memory");
		FAIL_EXIT(1);
	}
	if (!view.isContinuous()) {
		IMAGE_DEPTH_LOGE("[CapturePoints] view is discontinuous in memory");
		FAIL_EXIT(1);
	}
	if (!depth.isContinuous()) {
		IMAGE_DEPTH_LOGE("[CapturePoints] depth is discontinuous in memory");
		FAIL_EXIT(1);
	}
	const float * pW = (float*)(world.data);
	uint8_t * pV = (uint8_t*)(view.data);
	float * pD = (float*)(depth.data);

	// get the camera intrinsic parameters
	float fx, fy, cx, cy;
	if (m_cmm.type() == CV_64FC1) {
		fx = m_cmm.at<double>(0, 0);
		fy = m_cmm.at<double>(1, 1);
		cx = m_cmm.at<double>(0, 2);
		cy = m_cmm.at<double>(1, 2);
	}
	else if(m_cmm.type()==CV_32FC1) {
		fx = m_cmm.at<float>(0, 0);
		fy = m_cmm.at<float>(1, 1);
		cx = m_cmm.at<float>(0, 2);
		cy = m_cmm.at<float>(1, 2);
	}
	else {
		IMAGE_DEPTH_LOGE("[CapturePoints] m_cmm has invalid type");
		FAIL_EXIT(-1);
	}

	int nPt = world.size().width * world.size().height*4; // 4 channels
	int nVCol = view.size().width;
	int nVRow = view.size().height;

	float px, py, pz; // source 3d point in real world
	float ppx, ppy, ppz; // transformed into camera coordinate
	float sx, sy; // coordinates sx, sy on canvas, and depth sz.
	uint8_t r, g, b;

	if (!m_rot.isContinuous()) {
		IMAGE_DEPTH_LOGE("[CapturePoints] depth is discontinuous in memory");
		FAIL_EXIT(1);
	}
	const float *pR = (float*)(m_rot.data);

	for (int iPt = 0; iPt < nPt; iPt+=4) {
		px = pW[iPt];
		py = pW[iPt + 1];
		pz = pW[iPt + 2];

		// get the crossing point on canvas
		// first, we need to transform the points from real world coordinate
		// into camera coordinate, and then solve equation f(x, y, z=F)=0
		// for each 3d point with equation f be the line from optical center 
		// to the point with focus length be F.
		// the transformer is simple:
		// (x,y,z)(3D world) --> (x',y',z')(3D camera) --> (X,Y)(2D canvas)
		// the overall transform is P = K [R | t], x = P X
		// where K is 3x3 camera matrix, and R is camera rotation matrix,
		// t is the camera translation vector.
		
		// first apply translation
		px -= m_pos.x;
		py -= m_pos.y;
		pz -= m_pos.z;

		// then apply rotation
		ppx = pR[0] * px + pR[3] * py + pR[6] * pz;
		ppy = pR[1] * px + pR[4] * py + pR[7] * pz;
		ppz = pR[2] * px + pR[5] * py + pR[8] * pz;
		
		if (ppz > 0) { // only world before camera surface can be imaged
			// finally apply camera matrix, cast onto 2D canvas
			// never forget to normalize: (x, y, z) -> (x/z, y/z, 1)
			sx = std::roundf(ppx / ppz * fx + cx);
			sy = std::roundf(ppy / ppz * fy + cy);
			
			if (sx >= 0 && sx < nVCol && sy >= 0 && sy < nVRow) { // FOV
				int id_ = int(sy)*nVCol + int(sx);
				if (pD[id_] == 0 || ppz < pD[id_]) {
					pD[id_] = ppz;
					const uint8_t * pRGBA = (uint8_t*)(pW + (iPt + 3));
					id_ *= 3;
					pV[id_++] = pRGBA[0];
					pV[id_++] = pRGBA[1];
					pV[id_] = pRGBA[2];
				}
			}
		}
	}
	// save current frame to last frame buffer
	while (m_view_lock==TAKEN);
	m_view_lock = TAKEN;
	view.copyTo(m_view);
	depth.copyTo(m_view_depth);
	m_view_lock = FREE;
}


void r3d_depth_image::Camera::CaptureTriangles(
	const std::vector<Triangle> & tris, 
	cv::Mat & view) {
	// required to implement



}


// the rotation axis [vec] has two possible direction,
// and A could be negative
void r3d_depth_image::Camera::RotateAxis(cv::Point3f vec, float A) {
	cv::Mat rot_ = get_axis_rot(vec, A);

	// apply rotation matrix to the camera orientation vectors
	m_ort_x = rotate_vector(m_ort_x, rot_);
	m_ort_y = rotate_vector(m_ort_y, rot_);
	m_ort_z = rotate_vector(m_ort_z, rot_);
	
	// check if the three vectors forms a positive cross matrix
	if (CHECK_NONZERO(m_ort_x.dot(m_ort_y)) ||
		CHECK_NONZERO(m_ort_y.dot(m_ort_z)) ||
		CHECK_NONZERO(m_ort_z.dot(m_ort_x))
		) {
		IMAGE_DEPTH_LOGE("[RotateAxis] 3 ort vecs not pos-cross");
		FAIL_EXIT(-1);
	}

	// finally update the rotation matrix
	update_rot_by_orts(m_rot, m_ort_x, m_ort_y, m_ort_z);

	// compute [n1|n2|n3]^-1
	cv::Mat inv_;
	if (!cv::invert(m_rot, inv_, cv::DECOMP_LU)) {
		// this code will never be reached?
		IMAGE_DEPTH_LOGE("[RotateAxis] inv mat failed at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	if (!check_inversion(m_rot, inv_)) {
		IMAGE_DEPTH_LOGE("[RotateAxis] bad inv mat at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	inv_.copyTo(m_rot);
}


void r3d_depth_image::Camera::RotateAxis(
	const cv::Point3f & center,
	const cv::Point3f & axis,
	float degree
	) {
	// update camera position
	cv::Point3f pos_ = m_pos - center;
	cv::Mat rot_ = get_axis_rot(axis, degree);
	// rotate the camera position (z orientation)
	pos_ = rotate_vector(pos_, rot_);
	m_pos = pos_ + center; // convert back to real world coordinate

	// apply rotation matrix to the camera orientation vectors
	m_ort_x = rotate_vector(m_ort_x, rot_);
	m_ort_y = rotate_vector(m_ort_y, rot_);
	m_ort_z = rotate_vector(m_ort_z, rot_);

	// check if the three vectors forms a positive cross matrix
	if (CHECK_NONZERO(m_ort_x.dot(m_ort_y)) ||
		CHECK_NONZERO(m_ort_y.dot(m_ort_z)) ||
		CHECK_NONZERO(m_ort_z.dot(m_ort_x))
		) {
		IMAGE_DEPTH_LOGE("[RotateAxis] 3 ort vecs not pos-cross");
		FAIL_EXIT(-1);
	}

	// check if the target bound state is broken
	cv::Point3f ort_z = m_tar - m_pos;
	ASSERT_NONZERO(normalize(ort_z));
	ort_z -= m_ort_z;
	ASSERT_ZERO_VEC(ort_z);

	// finally update the rotation matrix
	update_rot_by_orts(m_rot, m_ort_x, m_ort_y, m_ort_z);

	// compute [n1|n2|n3]^-1
	cv::Mat inv_;
	if (!cv::invert(m_rot, inv_, cv::DECOMP_LU)) {
		// this code will never be reached?
		IMAGE_DEPTH_LOGE("[RotateAxis] inv mat failed at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	if (!check_inversion(m_rot, inv_)) {
		IMAGE_DEPTH_LOGE("[RotateAxis] bad inv mat at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	inv_.copyTo(m_rot);
}


void r3d_depth_image::Camera::LookAt(cv::Point3f target) {
	// update the camera orientation to ensure a direct pointing state
	// only rotate on the surface perpendicular to both z orientation
	// the target z orientation
	cv::Point3f tar_ort_z = target - m_pos;

	if (tar_ort_z == cv::Point3f(0, 0, 0)) {
		IMAGE_DEPTH_LOGE("[LookAt] target position as the same of camera");
		FAIL_EXIT(-1); 
	}
	normalize(tar_ort_z);

	m_tar = target;
	m_bound = true;

	cv::Point3f rot_axis = tar_ort_z.cross(m_ort_z);
	if (rot_axis == cv::Point3f(0, 0, 0)) return;
	if (CHECK_ZERO(normalize(rot_axis))) return;

	float n1 = rot_axis.x;
	float n2 = rot_axis.y;
	float n3 = rot_axis.z;

	// get the rotation angle [A] with cosA and sinA
	float cosA = m_ort_z.dot(tar_ort_z); // as both norm is 1
	float sinA = std::sqrtf(1 - cosA*cosA); // sign contained in axis vector

	// calculate the direct rotation matrix
	//           [cosA+n1^2*(1-cosA) n1*n2*(1-cosA)-n3*sinA n1*n3*(1-cosA)+n2*sinA]
	// R(n, A) = [n1*n2*(1-cosA)+n3*sinA cosA+n2^2*(1-cosA) n2*n3*(1-cosA)-n1*sinA]
	//	         [n1*n3*(1-cosA)-n2*sinA n2*n3*(1-cosA)+n1*sinA cosA+n3^2*(1-cosA)]
	cv::Mat rot_ = cv::Mat(cv::Size(3, 3), CV_32FC1);
	rot_.at<float>(0, 0) = cosA + n1*n1 * (1 - cosA);
	rot_.at<float>(1, 0) = n1*n2*(1 - cosA) - n3*sinA;
	rot_.at<float>(2, 0) = n1*n3*(1 - cosA) + n2*sinA;

	rot_.at<float>(0, 1) = n1*n2*(1 - cosA) + n3*sinA;
	rot_.at<float>(1, 1) = cosA + n2*n2 * (1 - cosA);
	rot_.at<float>(2, 1) = n2*n3*(1 - cosA) - n1*sinA;

	rot_.at<float>(0, 2) = n1*n3*(1 - cosA) - n2*sinA;
	rot_.at<float>(1, 2) = n2*n3*(1 - cosA) + n1*sinA;
	rot_.at<float>(2, 2) = cosA + n3*n3 * (1 - cosA);

	// apply rotation matrix to the camera orientation vectors
	m_ort_x = rotate_vector(m_ort_x, rot_);
	m_ort_y = rotate_vector(m_ort_y, rot_);
	m_ort_z = rotate_vector(m_ort_z, rot_);

	// check if the three vectors forms a positive cross matrix
	if (CHECK_NONZERO(m_ort_x.dot(m_ort_y)) ||
		CHECK_NONZERO(m_ort_y.dot(m_ort_z)) ||
		CHECK_NONZERO(m_ort_z.dot(m_ort_x))
		) {
		IMAGE_DEPTH_LOGE("[LookAt] 3 ort vecs not pos-cross");
		FAIL_EXIT(-1);
	}

	// finally update the rotation matrix
	// compose [n1|n2|n3]
	m_rot.create(cv::Size(3, 3), CV_32FC1);
	m_rot.at<float>(0, 0) = m_ort_x.x;
	m_rot.at<float>(0, 1) = m_ort_x.y;
	m_rot.at<float>(0, 2) = m_ort_x.z;

	m_rot.at<float>(1, 0) = m_ort_y.x;
	m_rot.at<float>(1, 1) = m_ort_y.y;
	m_rot.at<float>(1, 2) = m_ort_y.z;

	m_rot.at<float>(2, 0) = m_ort_z.x;
	m_rot.at<float>(2, 1) = m_ort_z.y;
	m_rot.at<float>(2, 2) = m_ort_z.z;

	// compute [n1|n2|n3]^-1
	cv::Mat inv_;
	if (!cv::invert(m_rot, inv_, cv::DECOMP_LU)) {
		// this code will never be reached?
		IMAGE_DEPTH_LOGE("[LookAt] inv mat failed at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	if (!check_inversion(m_rot, inv_)) {
		IMAGE_DEPTH_LOGE("[LookAt] bad inv mat at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	inv_.copyTo(m_rot);
}


void r3d_depth_image::Camera::Move(cv::Point3f vec) {
	// update the camera position
	m_pos += vec;
	// handle case in bound state
	if (m_bound) SetPosition(m_pos);
}


// [*] Disabled on bound state
void r3d_depth_image::Camera::Rotate(const cv::Mat & rot_) {
	if (m_bound) return;
	// apply rotation matrix to the camera orientation vectors
	m_ort_x = rotate_vector(m_ort_x, rot_);
	m_ort_y = rotate_vector(m_ort_y, rot_);
	m_ort_z = rotate_vector(m_ort_z, rot_);

	// check if the three vectors forms a positive cross matrix
	if (CHECK_NONZERO(m_ort_x.dot(m_ort_y)) ||
		CHECK_NONZERO(m_ort_y.dot(m_ort_z)) ||
		CHECK_NONZERO(m_ort_z.dot(m_ort_x))
		) {
		IMAGE_DEPTH_LOGE("[Rotate] 3 ort vecs not pos-cross");
		FAIL_EXIT(-1);
	}

	// finally update the rotation matrix
	// compose [n1|n2|n3]
	m_rot.create(cv::Size(3, 3), CV_32FC1);
	m_rot.at<float>(0, 0) = m_ort_x.x;
	m_rot.at<float>(0, 1) = m_ort_x.y;
	m_rot.at<float>(0, 2) = m_ort_x.z;

	m_rot.at<float>(1, 0) = m_ort_y.x;
	m_rot.at<float>(1, 1) = m_ort_y.y;
	m_rot.at<float>(1, 2) = m_ort_y.z;

	m_rot.at<float>(2, 0) = m_ort_z.x;
	m_rot.at<float>(2, 1) = m_ort_z.y;
	m_rot.at<float>(2, 2) = m_ort_z.z;

	// compute [n1|n2|n3]^-1
	cv::Mat inv_;
	if (!cv::invert(m_rot, inv_, cv::DECOMP_LU)) {
		// this code will never be reached?
		IMAGE_DEPTH_LOGE("[Rotate] inv mat failed at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	if (!check_inversion(m_rot, inv_)) {
		IMAGE_DEPTH_LOGE("[Rotate] bad inv mat at %d", __LINE__);
		FAIL_EXIT(-1);
	}

	inv_.copyTo(m_rot);
}


// YAW(along X axis), Pitch(along Y), ROLL(along Z)
// [*] Disabled on bound state
void r3d_depth_image::Camera::Yaw(float A) {
	if (m_bound) RotateAxis(m_tar, m_ort_x, -A);
	else RotateAxis(m_pos, m_ort_x, A);
}


// [*] Disabled on bound state
void r3d_depth_image::Camera::Pitch(float A) {
	if (m_bound) RotateAxis(m_tar, -m_ort_y, A);
	else RotateAxis(m_pos, m_ort_y, A);
}


// [*] Enabled also on bound state
void r3d_depth_image::Camera::Roll(float A) {
	RotateAxis(m_pos, m_ort_z, A);
}


// bring camera closer to the target along z orientation of itself
void r3d_depth_image::Camera::MoveAhead(float dist) {
	// under this situation, orientation remain unchanged
	cv::Point3f vec = dist * m_ort_z;
	m_pos += vec;
}


// bring camera right along x orientation of itself
void r3d_depth_image::Camera::MoveRight(float dist) {
	// move the camera left to the target
	cv::Point3f vec = dist * m_ort_x;
	m_pos += vec;
	if (m_bound) LookAt(m_tar);
}


// bring camera right along y orientation of itself
void r3d_depth_image::Camera::MoveUp(float dist) {
	cv::Point3f vec = dist * m_ort_y;
	m_pos += vec;
	if (m_bound) LookAt(m_tar);
}


r3d_depth_image::Camera::~Camera()
{
	//  nothing here
}
